.set noreorder
.set noat

.include "gtereg.inc"
.include "inline_s.inc"

.section .text.Square0
.global Square0
.type Square0, @function
Square0:
	# a0 - Pointer to input vector (v0)
	# a1 - Pointer to output vector (v1)

	lwc2	C2_IR1, 0($a0)
	lwc2	C2_IR2, 4($a0)
	lwc2	C2_IR3, 8($a0)

	nSQR(0)

	swc2	C2_IR1, 0($a1)
	swc2	C2_IR2, 4($a1)
	swc2	C2_IR3, 8($a1)

	jr		$ra
	nop

.section .text.VectorNormalS
.global VectorNormalS
.type VectorNormalS, @function
VectorNormalS:
	
	lw		$t0, 0($a0)
	lw		$t1, 4($a0)
	lw		$t2, 8($a0)
	
	mtc2	$t0, C2_IR1
	mtc2	$t1, C2_IR2
	mtc2	$t2, C2_IR3
	
	nSQR(0)
	
	mfc2	$t3, C2_MAC1
	mfc2	$t4, C2_MAC2
	mfc2	$t5, C2_MAC3
	
	add		$t3, $t4
	add		$v0, $t3, $t5
	mtc2	$v0, C2_LZCS
	nop
	nop
	mfc2	$v1, C2_LZCR
	
	addiu	$at, $0 , -2
	and		$v1, $at
	
	addiu	$t6, $0 , 0x1f
	sub		$t6, $v1
	sra		$t6, 1
	addiu	$t3, $v1, -24
	
	bltz	$t3, .Lvalue_neg
	nop
	b		.Lvalue_pos
	sllv	$t4, $v0, $t3
.Lvalue_neg:
	addiu	$t3, $0 , 24
	sub		$t3, $v1
	srav	$t4, $v0, $t3
.Lvalue_pos:
	addi	$t4, -64
	sll		$t4, 1
	
	la		$t5, _norm_table
	addu	$t5, $t4
	lh		$t5, 0($t5)
	
	mtc2	$t0, C2_IR1
	mtc2	$t1, C2_IR2
	mtc2	$t2, C2_IR3
	mtc2	$t5, C2_IR0
	
	nGPF(0)
	
	mfc2	$t0, C2_MAC1
	mfc2	$t1, C2_MAC2
	mfc2	$t2, C2_MAC3
	
	sra		$t0, $t6
	sra		$t1, $t6
	sra		$t2, $t6
	
	sh		$t0, 0($a1)
	sh		$t1, 2($a1)
	jr		$ra
	sh		$t2, 4($a1)

.section .data._norm_table
.type _norm_table, @object
_norm_table:
	.hword 0x1000, 0x0fe0, 0x0fc1, 0x0fa3, 0x0f85, 0x0f68, 0x0f4c, 0x0f30
	.hword 0x0f15, 0x0efb, 0x0ee1, 0x0ec7, 0x0eae, 0x0e96, 0x0e7e, 0x0e66
	.hword 0x0e4f, 0x0e38, 0x0e22, 0x0e0c, 0x0df7, 0x0de2, 0x0dcd, 0x0db9
	.hword 0x0da5, 0x0d91, 0x0d7e, 0x0d6b, 0x0d58, 0x0d45, 0x0d33, 0x0d21
	.hword 0x0d10, 0x0cff, 0x0cee, 0x0cdd, 0x0ccc, 0x0cbc, 0x0cac, 0x0c9c
	.hword 0x0c8d, 0x0c7d, 0x0c6e, 0x0c5f, 0x0c51, 0x0c42, 0x0c34, 0x0c26
	.hword 0x0c18, 0x0c0a, 0x0bfd, 0x0bef, 0x0be2, 0x0bd5, 0x0bc8, 0x0bbb
	.hword 0x0baf, 0x0ba2, 0x0b96, 0x0b8a, 0x0b7e, 0x0b72, 0x0b67, 0x0b5b
	.hword 0x0b50, 0x0b45, 0x0b39, 0x0b2e, 0x0b24, 0x0b19, 0x0b0e, 0x0b04
	.hword 0x0af9, 0x0aef, 0x0ae5, 0x0adb, 0x0ad1, 0x0ac7, 0x0abd, 0x0ab4
	.hword 0x0aaa, 0x0aa1, 0x0a97, 0x0a8e, 0x0a85, 0x0a7c, 0x0a73, 0x0a6a
	.hword 0x0a61, 0x0a59, 0x0a50, 0x0a47, 0x0a3f, 0x0a37, 0x0a2e, 0x0a26
	.hword 0x0a1e, 0x0a16, 0x0a0e, 0x0a06, 0x09fe, 0x09f6, 0x09ef, 0x09e7
	.hword 0x09e0, 0x09d8, 0x09d1, 0x09c9, 0x09c2, 0x09bb, 0x09b4, 0x09ad
	.hword 0x09a5, 0x099e, 0x0998, 0x0991, 0x098a, 0x0983, 0x097c, 0x0976
	.hword 0x096f, 0x0969, 0x0962, 0x095c, 0x0955, 0x094f, 0x0949, 0x0943
	.hword 0x093c, 0x0936, 0x0930, 0x092a, 0x0924, 0x091e, 0x0918, 0x0912
	.hword 0x090d, 0x0907, 0x0901, 0x08fb, 0x08f6, 0x08f0, 0x08eb, 0x08e5
	.hword 0x08e0, 0x08da, 0x08d5, 0x08cf, 0x08ca, 0x08c5, 0x08bf, 0x08ba
	.hword 0x08b5, 0x08b0, 0x08ab, 0x08a6, 0x08a1, 0x089c, 0x0897, 0x0892
	.hword 0x088d, 0x0888, 0x0883, 0x087e, 0x087a, 0x0875, 0x0870, 0x086b
	.hword 0x0867, 0x0862, 0x085e, 0x0859, 0x0855, 0x0850, 0x084c, 0x0847
	.hword 0x0843, 0x083e, 0x083a, 0x0836, 0x0831, 0x082d, 0x0829, 0x0824
	.hword 0x0820, 0x081c, 0x0818, 0x0814, 0x0810, 0x080c, 0x0808, 0x0804
